﻿using DroneLib.Communication;
using System;
using System.Collections.Generic;
using System.Text;
using System.Threading.Tasks;
using static MAVLink;

namespace DroneLib.Vehicles
{
    public abstract class Plane : Vehicle
    {

        internal Plane(byte sysId, MavStream link)
           : base(sysId, link)
        {

        }

        public bool Landed => _SystemState == (byte)MAVLink.MAV_STATE.STANDBY;

        public abstract Task<bool> Land();

        public abstract Task<bool> Loiter();

        public abstract Task<bool> AutoTune();

        //public abstract bool Goto(float x, float y, float z, MAVLink.MAV_FRAME frae = MAVLink.MAV_FRAME.GLOBAL_INT);

        public  async Task<bool> TakeOff(int altitude, int pitch = 0)
        {
            if (this.Armed == false)
                return false;

            bool b;
            if (this.Firmware == DroneLib.Vehicle.Firmwares.ArduCopter2)
            {
                b = await this.SetMode(this.GetModeCode("GUIDED"));
                if (b == false)
                    return b;

                return await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.TAKEOFF, 50000, pitch, 0, 0, 0, 0, 0, altitude) == MAV_RESULT.ACCEPTED;
            }
            else if(this.Firmware== DroneLib.Vehicle.Firmwares.ArduPlane)
            {
                b = await this.WriteParameter("TKOFF_ALT", altitude);
                if (b == false)
                    return false;
                b = await this.WriteParameter("TKOFF_LVL_PITCH", pitch);
                if (b == false)
                    return false;
                b = await this.SetMode((UInt32)PLANE_MODE.TAKEOFF);
                return b;
            }

            return false;
        }
    }
}
